/**
 * @file ebex_sort.c
 *
 * @date Jan 21, 2008
 * @author seth
 *
 * @brief This file is part of EBEX State Estimator, created for the EBEX project
 *
 * This software is copyright (C) 2008 Columbia University
 *
 * EBEX State Estimator is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * EBEX State Estimator is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with EBEX State Estimator; if not, write to the Free Software Foundation, Inc.,
 * 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 *
 */

#include <stddef.h>
#include <memory_util.h>
#include <ebex_sort.h>

typedef enum
{
	swap_type_char,
	swap_type_long,
	swap_type_multiple
}swaptype_t;

typedef int (*comparison_fn_t) (const void *, const void *);

static int cmp_double(const void *m_data1, const void *m_data2)
{
	double a = *(double*)m_data1;
	double b = *(double*)m_data2;

	return (a < b ? -1:(a > b ? 1 : 0));
}

static int cmp_fitness(const void *m_data1, const void *m_data2)
{
	double a = (*((organism_t**)m_data1))->fitness;
	double b = (*((organism_t**)m_data2))->fitness;

	return (a < b ? 1:(a > b ? -1 : 0));
}

#define swapcode(TYPE, parmi, parmj, n) do {            \
        long i = (n) / sizeof (TYPE);                   \
        register TYPE *pi = (TYPE *) (parmi);           \
        register TYPE *pj = (TYPE *) (parmj);           \
        do {                                            \
                register TYPE   t = *pi;                \
                *pi++ = *pj;                            \
                *pj++ = t;                              \
        } while (--i > 0);                              \
} while (0)


#define SET_SWAP_TYPE(a, es) 								\
	swaptype = (((char *)a - (char *)0) | es) % sizeof(long) ? 2 : (es == sizeof(long)) ? 0 : 1;

static __inline void
swapfunc(char *a, char *b, int n, swaptype_t m_swaptype)
{
        if (m_swaptype != swap_type_char)
                swapcode(long, a, b, n);
        else
                swapcode(char, a, b, n);
}

#define eswap(a, b, type) do {								\
        if ((type) == swap_type_long) {						\
                long t = *(long *)(a);						\
                *(long *)(a) = *(long *)(b);				\
                *(long *)(b) = t;							\
        } else												\
                swapfunc(a, b, m_element_size, type);		\
} while (0)

#define vecswap(a, b, n) if ((n) > 0) swapfunc(a, b, n, swaptype)

static __inline char *
med3(char *a, char *b, char *c, comparison_fn_t m_cmpfn)
{
        return m_cmpfn(a, b) < 0 ?
                (m_cmpfn(b, c) < 0 ? b : (m_cmpfn(a, c) < 0 ? c : a))
                : (m_cmpfn(b, c) > 0 ? b : (m_cmpfn(a, c) < 0 ? a : c));
}

#ifndef min
#define min(a, b) (((a) < (b)) ? (a) : (b))
#endif

static inline void better_insertion_sort(char *m_array, size_t m_element_size, size_t m_size, comparison_fn_t m_cmpfn, swaptype_t m_type)
{
	char *low_element;
	char *high_element;
	
	for (low_element = m_array + m_element_size; low_element < m_array + m_size * m_element_size; low_element += m_element_size)
	{
		for (high_element = low_element; high_element > m_array && m_cmpfn(high_element - m_element_size, high_element) > 0; high_element -= m_element_size)
		{
			eswap(high_element, high_element - m_element_size, m_type);
		}
	}
}

/**
 * This sort function is a modified version of that given by Benson and McIlroy in
 * SOFTWARE—PRACTICE AND EXPERIENCE, VOL. 23(11), 1249–1265 (NOVEMBER 1993).
 * @param m_array
 * @param m_size
 * @param m_element_size
 * @param m_cmpfn
 */
static inline void better_qsort(void *m_array, size_t m_size, size_t m_element_size, comparison_fn_t m_cmpfn)
{
	char *pa, *pb, *pc, *pd;
	char *pl, *pm, *pn;
	int d, r, swap_cnt;
	register char *a = m_array;
	swaptype_t swaptype;

	do
	{
		swaptype = (((char *)a - (char *)0) | (uintptr_t) m_element_size) % sizeof(long) ? swap_type_char : (m_element_size == sizeof(long)) ? swap_type_long : swap_type_multiple;
		swap_cnt = 0;
		
		/// If we have fewer than 7 elements, then do a simple insertion sort
		if (m_size < 7)
		{
			better_insertion_sort(a, m_element_size, m_size, m_cmpfn, swaptype);
			return;
		}
		
		pm = (char *) a + (m_size / 2) * m_element_size;
		if (m_size > 7)
		{
			pl = (char *) a;
			pn = (char *) a + (m_size - 1) * m_element_size;
			if (m_size > 40)
			{
				d = (m_size / 8) * m_element_size;
				pl = med3(pl, pl + d, pl + 2 * d, m_cmpfn);
				pm = med3(pm - d, pm, pm + d, m_cmpfn);
				pn = med3(pn - 2 * d, pn - d, pn, m_cmpfn);
			}
			pm = med3(pl, pm, pn, m_cmpfn);
		}
		eswap(a, pm, swaptype);
		pa = pb = (char *) a + m_element_size;

		pc = pd = (char *) a + (m_size - 1) * m_element_size;
		
		while(1)
		{
			while (pb <= pc && (r = m_cmpfn(pb, a)) <= 0)
			{
				if (r == 0)
				{
					swap_cnt = 1;
					eswap(pa, pb, swaptype);
					pa += m_element_size;
				}
				pb += m_element_size;
			}
			while (pb <= pc && (r = m_cmpfn(pc, a)) >= 0)
			{
				if (r == 0)
				{
					swap_cnt = 1;
					eswap(pc, pd, swaptype);
					pd -= m_element_size;
				}
				pc -= m_element_size;
			}
			if (pb > pc)
				break;

			eswap(pb, pc, swaptype);
			swap_cnt = 1;
			pb += m_element_size;
			pc -= m_element_size;
		}
		
		/// No swaps means that we are in an inefficient qsort configuration (almost sorted?).  Switch to the light insertion sort
		if (swap_cnt == 0)
		{
			better_insertion_sort(a, m_element_size, m_size, m_cmpfn, swaptype);
			return;
		}

		pn = (char *) a + m_size * m_element_size;
		r = min(pa - (char *)a, pb - pa);
		vecswap(a, pb - r, r);
		r = min((long)(pd - pc), (long)(pn - pd - m_element_size));
		vecswap(pb, pn - r, r);
		r = pb - pa;
		if (r > m_element_size)
			better_qsort(a, r / m_element_size, m_element_size, m_cmpfn);
		r = pd - pc;
		if (r > m_element_size)
		{
			/* Iterate rather than recurse to save stack space */
			a = pn - r;
			m_size = r / m_element_size;
		}
	} while (r > m_element_size);
}

/**
 * External interface to the sorting function for a double array
 * @param m_array Pointer to the double array
 * @param m_size Length of the double array
 */
void sort_double(double *m_array, size_t m_size)
{
    better_qsort(m_array, m_size, sizeof(double), cmp_double);
}

/**
 * External interface to the sorting function for a population.  Will update
 * the #m_pop->sorted_organisms array in the population
 * @param m_pop Pointer to the population
 */
void sort_population(population_t *m_pop)
{
	better_qsort(m_pop->sorted_organisms, m_pop->size, sizeof(*m_pop->sorted_organisms), cmp_fitness);
}

